
#pragma once

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp> //显示机器人导航中的路径相关的数据
#include <geometry_msgs/msg/pose_stamped.hpp> //以箭头或坐标轴的方式绘制位姿
#include <geometry_msgs/msg/twist.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <memory>
#include <vector>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <math.h>
#include <fstream>
#include <iostream>

#include "motion_controller/pid.hpp"
#include "base_interface/msg/trajectory_custom.hpp"
#include "base_interface/msg/ops.hpp"
#include "base_interface/msg/control_cmd.hpp"

#define PI 3.1415926
#define AXIS_LENGTH 0.4784 // 线速度 = 角速度*半径
#define WHEEL_RADIUS 0.08  // 轮子半径 m
#define delt_t 0.005

using namespace std;
using namespace std::chrono_literals; // 可以使用带单位的时间量 比如下面用的1s 500ms
using namespace Eigen;

struct ControlMsg
{
  float PPos_x; // 位置索引指向的x,y坐标 (实际位置)
  float PPos_y;
float PPos_vx;
float PPos_vy;

  float TPos_x; // 时间索引指向的x,y坐标 (当前时刻应该到达的位置)
  float TPos_y;
  
  float Tar_Px;
  float Tar_Py;

  float Tar_Vx; // 时间索引指向的vx,vy    期望速度，可以理解为前馈
  float Tar_Vy;

  float to_P_pid_vx;
  float to_P_pid_vy;

  float to_V_pid_vx;
  float to_V_pid_vy;

  float out_vx;
  float out_vy;

  float OUT_Corr;
};

class Motion_Controller : public rclcpp ::Node
{
public:
  Motion_Controller();
  ~Motion_Controller(){};

  void ComputeCtrl(const float &pos_x, const float &pos_y,
                                      const float &vel_x, const float &vel_y,
                                      const MatrixXd& PosVel_Matrix,
                                      const float &time_past_real,
                                      const float &ops_yaw,
                                      bool clear_pre_pos_flag);

  void Chassis_Speed_Decomposition(float cmd_vel_, float cmd_vel_angle_, float ops_yaw_, float omega);

  void Rviz_RobotTraj(float real_x, float real_y, float tar_x, float tar_y);
  
private:
  // 合速度控制量
  float cmd_vel;       // 合速度大小
  float cmd_vel_angle; // 合速度方向

  // 开启定时器，测试电机sin曲线用
  rclcpp::TimerBase::SharedPtr timer_;
  void test_timecallback();

  // 订阅规划模块传递的轨迹
  rclcpp::Subscription<base_interface::msg::TrajectoryCustom>::SharedPtr recTraj;
  void TrajCallback(const base_interface::msg::TrajectoryCustom::SharedPtr traj_custom);

  // 订阅下位机传递的信息，比如坐标位置等等
  rclcpp::Subscription<base_interface::msg::Ops>::SharedPtr recSlaveMsg;
  void RecSlaveMsg(base_interface::msg::Ops::SharedPtr slave_msg);

  // 发布上位机minipc计算的控制指令
  rclcpp::Publisher<base_interface::msg::ControlCmd>::SharedPtr pub_Cmdvel;

  // 发布机器人实时位置到rviz2进行可视化
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_robot_traj;

  visualization_msgs::msg::Marker traj_tar;    //规划的轨迹(目标位置轨迹)
  visualization_msgs::msg::Marker traj_real;   //实际的轨迹（实际位置轨迹）


  std::shared_ptr<PidCompute> pos_pid_ptr; // 位置环pid
  std::shared_ptr<PidCompute> vel_pid_ptr; // 速度环pid

  /*---data structure （存放每隔0.005s的轨迹的位置速度数据的矩阵）---*/
  MatrixXd PosVel_Matrix;
  ControlMsg Target_Msg;
  int pos_index;   // 位置索引
  int time_index;  // 时间索引
  int tar_index;
  float decayer;   // 衰减比

  float error_spin;
  float error_spin_last;
  float ops_yaw_last = 0.0;

  float delt_s; // delt_t时间间隔车移动的距离

  float pos_x_last;
  float pos_y_last;

  float motion_length;   // 从起点到当前时刻车移动的实际总距离
  float plan_length;     // 从起点到当前时刻车移动的期望总距离

  bool strat_TimeStamp_flag = 0; // 每次新的轨迹跟踪任务开始的标志位  目的是记录下新的轨迹跟踪任务开始时对应的timestamp
  rclcpp::Time new_Trak_task_stamp;

  bool traj_Trak_flag = 0;
  long count = 0; // 接收规划模块传递的轨迹第count次

  vector<float> plan_length_index; // 从规划的起点到第i个点的路程长度

  // Eigen::Vector4d end_expect_state =Eigen::Vector4d::Zero();
};

/**
 * @brief 保留小数点后的数字的个数
 */
float NumDecimal(float value, int n_);

/**
 * @brief 限幅
*/
void SetBound(float& value, float max);